#ifndef MATHS_H
#define MATHS_H

#include "Vector3D.h"

#ifndef PI
#define PI 3.14159265358979323846
#endif

typedef Vector3D Vector;
typedef double Scalar;
//typedef double Angle;

//Basically just a wrapper for a vector but also has an angle
//Entities are attached to these
class Point
{
public:
    
    //TODO: Decide if this constructor should be private
    
    Point(Vector vPos, Scalar fAngle=0) : m_vPos(vPos), m_fAngle(fAngle)      {}
    void setPos(Vector vVal)                    { m_vPos = vVal; }
    Vector& getPos()                            { return m_vPos; }
    void setAngle(Scalar fVal)                  { m_fAngle = fVal; }
    Scalar& getAngle()                          { return m_fAngle; }
protected:
    Vector m_vPos;
    Scalar m_fAngle;
};

class Segment
{
    
public:
    
    //Segment(Vector vPos1, Vector vPos2) : m_pPoint1(0), m_pPoint2(0)
    Segment() : m_pPoint1(0), m_pPoint2(0)         {}
    Segment(Point* pPoint1, Point* pPoint2) : m_pPoint1(0), m_pPoint2(0)
    {
        
        //TODO: Decide if should create new points on the heap or not
        
        init(pPoint1, pPoint2);
        
    }
    
    Point* getPoint1()         { return m_pPoint1; }
    Point* getPoint2()         { return m_pPoint2; }
    
    bool isSet()                { return m_pPoint1 && m_pPoint2; }
    Scalar getAngle()           { return (m_pPoint2->getPos() - m_pPoint1->getPos()).getAngle(); }
    Scalar getLength()          { return (m_pPoint2->getPos() - m_pPoint1->getPos()).getLength(); }
    Vector getVector()          { return m_pPoint2->getPos() - m_pPoint1->getPos(); }
    Vector getStart()           { return m_pPoint1->getPos(); }
    Vector getEnd()             { return m_pPoint2->getPos(); }
    
private:
    
    void init(Point* pPoint1, Point* pPoint2)
    {
        m_pPoint1 = pPoint1;
        m_pPoint2 = pPoint2;
    }
    
    Point* m_pPoint1;
    Point* m_pPoint2;
    
};

#endif // MATHS_H
